/*client*/

#include <stdio.h>
#include <conio.h>
#include <winsock.h>
#include <iostream>
#include "KinectMsg.h"
#include "Kinect.h"
using namespace std;

static const int BUFSIZE = 5120;
Kinect *kinect;

DWORD WINAPI UDPStraming(void *pArg) {
	struct hostent *hp;
	struct sockaddr_in saddr, caddr;
	int sock, len, fromlen;
	int port = 21652;
	char *dest = "192.168.1.103";

	WSADATA WsaDat;
	if (WSAStartup(MAKEWORD(2,2), &WsaDat) != 0) {
		cerr<<"Couldn't Init"<<endl;
		getch();
		exit(-1);
	}

	if((sock = socket(AF_INET, SOCK_DGRAM,0)) <0) {
		cerr<<"sock create failed"<<endl;
		getch();
		exit(-1);
	}
	static const int MSGSIZE = sizeof(KinectMsg);

	if((hp = gethostbyname(dest)) == NULL) {
		cerr<<"can't get host name"<<endl;
		getch();
		exit(-1);
	}
	memset(&saddr, 0, sizeof(saddr));
	memset(&caddr, 0, sizeof(caddr));
	saddr.sin_family = AF_INET;
	saddr.sin_addr.S_un .S_un_b.s_b1 = (unsigned char)hp->h_addr_list[0][0];
	saddr.sin_addr.S_un .S_un_b.s_b2 = (unsigned char)hp->h_addr_list[0][1];
	saddr.sin_addr.S_un .S_un_b.s_b3 = (unsigned char)hp->h_addr_list[0][2];
	saddr.sin_addr.S_un .S_un_b.s_b4 = (unsigned char)hp->h_addr_list[0][3];
	saddr.sin_port = htons(port);
	KinectMsg *msg = new KinectMsg();
	printf("size %d, %d, %d, %d\n", MSGSIZE, sizeof(msg->depthMD), sizeof(msg->sceneMD),sizeof(msg->body));
	char *tmp = new char[BUFSIZE+2];
	while(1) {
		*msg = *(kinect->getKinectMsg());
		char *buf = reinterpret_cast<char*>(msg);
		long totalSend = 0;
		int i;
		for(i = 0; i<(MSGSIZE/BUFSIZE)+1; i++) {
			*(reinterpret_cast<unsigned short*>(tmp)) = (unsigned short)i;
			/*cout<<i<<",";
			if(i%20 == 0) {
				cout<<endl;
			}*/
			memcpy(tmp+2,buf+(i*BUFSIZE), (i==MSGSIZE/BUFSIZE?sizeof(BodyInfo):BUFSIZE));
			if(i ==  MSGSIZE/BUFSIZE) {	//we send all previouse package, only bodyinfo left
				len = sendto(sock, tmp, sizeof(BodyInfo)+2, 0,(LPSOCKADDR)&saddr,sizeof(saddr));
			} else {
				len = sendto(sock,tmp, BUFSIZE+2, 0,(LPSOCKADDR)&saddr,sizeof(saddr));
			}
			len -= 2;
			if(len <= 0) {
				cout<<"package "<<i<<" send failed!"<<endl;
				break;
			}
			if(len < BUFSIZE && i != MSGSIZE/BUFSIZE) {
				cout<<"package "<<i<<" only send "<<len<<endl;
			}
			totalSend += len;
			Sleep(5);
		}
		cout<<"i = "<<i<<", Total Send "<<totalSend<<endl;
		Sleep(100);
	}

}


int main() {
	DWORD tid;
	HANDLE _handle = CreateThread(0,
		0,
		UDPStraming,
		NULL,
		CREATE_SUSPENDED,
		&tid);

	kinect = new Kinect(1);
	kinect->initKinect();
	kinect->updateBodyInfo();
	ResumeThread(_handle);
	//getch();
	while(1) {
		kinect->updateBodyInfo();
		BodyInfo body = kinect->getBodyInfo();
		if(body.bTracking && body.bRHand) {
			//ResumeThread(_handle);
			//printf("RHand %.0f, %.0f, %.0f\n", body.pRHand.X, body.pRHand.Y, body.pRHand.Z);
			//break;
		}
	}
	getch();
	return 0;
}

